%%%  AttitudeControl_ADRC_Sim
clear
path('./icon/',path);
%Run the model initialization file icon/init.m
Init;

%add disturb to the system
DisturbOn = 1; %关闭扰动
DisturbStartTime = 3; %扰动开始时间

%Constant value
RAD2DEG = 57.2957795;
DEG2RAD = 0.0174533;
%throttle when UAV is hovering
THR_HOVER = 0.3

%% POS control
% max control angle, default 35deg
MAX_CONTROL_ANGLE_ROLL = 35;
MAX_CONTROL_ANGLE_PITCH  = 35;
% max control angle rate, rad/s 
MAX_CONTROL_ANGLE_RATE_PITCH = 20;
MAX_CONTROL_ANGLE_RATE_ROLL = 20;
MAX_CONTROL_ANGLE_RATE_Y = 200;
% max control speed, m/s
MAX_CONTROL_VELOCITY_XY = 5;
MAX_CONTROL_VELOCITY_Z = 3;
% throttle amplitude
MAX_MAN_THR = 0.9;
MIN_MAN_THR = 0.05;

% integral saturation
Saturation_I_RP_Max = 0.4;
Saturation_I_RP_Min = -0.4;
Saturation_I_Y_Max = 0.2;
Saturation_I_Y_Min = -0.2;
Saturation_I_ah = 3.43;
Saturation_I_az = 5;

% position PID parameters
Kpxp = 1.0;
Kpyp = 1.0;
Kpzp = 4.0;
Kvxp = 2.5; Kvxi = 0.4; Kvxd = 0.01;
Kvyp = 2.5; Kvyi = 0.4; Kvyd = 0.01;
Kvzp = 0.45; Kvzi = 0.2; Kvzd = 0.1;

%% Initial condition
ModelInit_PosE = [0, 0, 0];
ModelInit_VelB = [0, 0, 0];
ModelInit_AngEuler = [0, 0, 0];
ModelInit_RateB = [0, 0, 0];
ModelInit_Rads = 0;
%% control parameter
%Attitude PID parameters
Kp_YAW_AngleRate = 0.5;
Ki_YAW_AngleRate = 0.01;
Kd_YAW_AngleRate = 0.00;
%integral saturation
% Saturation_I_RP_Max = 0.3;
% Saturation_I_RP_Min = -0.3;
% Saturation_I_Y_Max = 0.2;
% Saturation_I_Y_Min = -0.2;
%max control angle,default 35deg
% MAX_CONTROL_ANGLE_ROLL = 20;
% MAX_CONTROL_ANGLE_PITCH  = 20;
% % %max control angle rate,rad/s 
% MAX_CONTROL_ANGLE_RATE_PITCH = 220;
% MAX_CONTROL_ANGLE_RATE_ROLL = 220;
% MAX_CONTROL_ANGLE_RATE_Y = 200;
%parameters in adrc
SL_TD_R = single(100.0);
SL_TD_H = single(1);

SL_NLSEF_R = single(200);%200
SL_NLSEF_H = single(25.0);%25
SL_NLSEF_C = single(5);%5

SL_ESO_B01 = single(200.0);%800
SL_ESO_B02 = single(3000.0);%10000
SL_ESO_B03 = single(8000.0);%5000

SL_ESO_B0 = single(100);

SL_ESO_H = single(1);
SL_ESO_D = single(0.01);%0.01：2.5T

Ts=0.00125;

tau2sigma = single(0.108);
%CSC
% RC1_MIN = single(1050);
% RC1_MAX = single(1950);
% RC2_MIN = single(1050);
% RC2_MAX = single(1950);
% RC3_MIN = single(1050);
% RC3_MAX = single(1950);
% RC4_MIN = single(1050);
% RC4_MAX = single(1950);
%% run simulink model
%AttitudeControl_ADRC_Sim

%%SMC
k = 1;
c1 = 35/7*k;
k1 = 10*k;
k2 = 10*k;
k3 = k;


%% pid
Kp_ROLL_ANGLE = 6.5;
Kp_ROLL_AngleRate = 0.3;%0.15
Ki_ROLL_AngleRate = 0.3;%0.2
Kd_ROLL_AngleRate = 0.005;%0.005